% References:
% [1] Terminal Guidance for a Hypersonic Vehicle with Impact Time Control 
% [2] 宋瑞 硕士毕业论文第五章 考虑多约束的协同末制导方法
% [3] 郭宇恒 博士毕业论文第四章 数据驱动的攻击时间控制末制导
% [4] 李东旭.多高超声速导弹协同末制导律及可行初始位置域研究
% PNG、ITCG 方法对比，包括PNG、ITCG1、ITCG2、ITCG3
clc
clear
clear global
global hInt hGui N1 N2 ku kv kr kdelta Td T0 d2r mass g S 
global alphamin alphamax alphaRateLimit bankRateLimit
hInt = 0.01; % 积分步长，h=0.1时控制量抖动过于剧烈，故设为0.01
hGui = 0.1; % 制导周期
hk = hGui/hInt;
d2r = pi/180;
N1 = 3;
N2 = 3;
ku = 8; % 时间误差比例系数
kv = 2; % 时间偏置项系数
kr = 5e-6; % 欧拉法更新剩余时间系数
kdelta = 5; % 偏置项速度前置角系数
g = 9.81;
alphaRateLimit = 5*pi/180;
bankRateLimit = 40*pi/180;
alphamin = -1*pi/180;
alphamax = 20*pi/180;
mass = 1600;
S = 0.5026;
%% case
% case 1
state0 = [25e3 -30e3  20e3 1600 5*d2r -30*d2r];
% state0 = [25e3  30e3  25e3 1600 2*d2r -170*d2r];
% state0 = [-25e3 -30e3  20e3 1600 5*d2r 0*d2r];
% case 2
% state0 = [25e3 -30e3  20e3 1550 2*d2r -40*d2r];
% case 3
% state0 = [25e3 -30e3  20e3 1500 1*d2r -50*d2r];
% case 4 guo
% state0 = [-20e3 -55e3 30e3 1600 -10*d2r 5*d2r];
%% PNG
% res: t x y z V fpa azi lambda phi tPNG alphad bankd et r dr delta eta
num = 6e3;
res = nan(num,17);
res(1, 1:7) = [0 state0];
res(1, 11:13) = [0 0 0];
for i = 2:num 
    res_past = res(i-1,:);
    res_new = PNGguidanceloop(res_past);
%     [res_new(11), res_new(12)] = limitdegrate(res_new(11), res_new(12), res_past(11), res_past(12));
    res(i,:) = res_new;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
PNG = res(1:num, :);
T0 = num*hInt;
save('data.mat', 'PNG', '-append');
%% ITCG1
Td = round(T0 + 4);
hkv = 10;
% res: t x y z V fpa azi lambda phi tPNG alphad bankd et r dr delta eta
num = Td/hInt;
res = nan(num,17);
res(1, 1:7) = [0 state0];
res(1, 10) = T0;
res(1, 11:13) = [0 0 0];
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res_new = ITCGguidanceloop(res_past);
%         [res_new(11), res_new(12)] = limitdegrate(res_new(11), res_new(12), res_past(11), res_past(12));
        hkv = 0;
    else
        res_new = ITCGloop(res_past);
    end
%     res_new(11) = limitRate(res_new(11), res_past(11), hInt, 8*pi/180);
%     res_new(12) = limitRate(res_new(12), res_past(12), hInt, 45*pi/180);
    res(i,:) = res_new;
    hkv = hkv + 1;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
ITCG1 = res(1:num, :);
save('data.mat', 'ITCG1', '-append');
%% ITCG2
Td = round(T0 + 3);
num = (Td+1)/hInt;
hkv = 0;
% res: t x y z V fpa azi lambda phi tPNG alphad bankd et r dr delta eta
res = nan(num,17);
res(1, 1:7) = [0 state0];
res(1, 10) = T0;
res(1, 11:13) = [0 0 0];
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res(i,:) = ITCGguidanceloop(res_past);
        hkv = 0;
    else
        res(i,:) = ITCGloop(res_past);
    end
    hkv = hkv + 1;
    if res(i, 4) <= 0
        num = i;
        break;
    end
end
ITCG2 = res(1:num, :);
save('data.mat', 'ITCG2', '-append');
%% ITCG3
Td = round(T0 + 6);
num = (Td+1)/hInt;
hkv = 0;
% res: t x y z V fpa azi lambda phi tPNG alphad bankd et r dr delta eta
res = nan(num,17);
res(1, 1:7) = [0 state0];
res(1, 10) = T0;
res(1, 11:13) = [0 0 0];
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res(i,:) = ITCGguidanceloop(res_past);
        hkv = 0;
    else
        res(i,:) = ITCGloop(res_past);
    end
    hkv = hkv + 1;
    if res(i, 4) < 20
        num = i;
        break;
    end
end
ITCG3 = res(1:num, :);
save('data.mat', 'ITCG3', '-append');